import numpy as np

def degreeToRadians(x):
    return x / 360 * 2 * np.pi

def radiansToDegree(x):
    return 360 * (x / (2 * np.pi))

def getRotationMatrix(phi, theta, gamma):
    """
    Args:
        phi: degree 绕x轴旋转的角度
        theta: degree 绕y轴旋转的角度
        gamma: degree 绕z轴旋转的角度

    Returns: RotationMatrix
    """
    r11 = np.cos(gamma) * np.cos(theta)
    r12 = np.cos(gamma) * np.sin(phi) * np.sin(theta) - np.cos(phi) * np.sin(gamma)
    r13 = np.sin(phi) * np.sin(gamma) + np.cos(phi) * np.cos(gamma) * np.sin(theta)
    r21 = np.cos(theta) * np.sin(gamma)
    r22 = np.cos(phi) * np.cos(gamma) + np.sin(phi) * np.sin(gamma) * np.sin(theta)
    r23 = np.cos(phi) * np.sin(gamma) * np.sin(theta) - np.cos(gamma) * np.sin(phi)
    r31 = -np.sin(theta)
    r32 = np.cos(theta) * np.sin(phi)
    r33 = np.cos(phi) * np.cos(theta)
    return np.mat([[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]],
                  dtype=np.float32)


# 相机光心在世界坐标系中的位置
Camera_position_x = 600
Camera_position_y = -600
Camera_position_z = -1000
# 相机在世界坐标系中旋转轴
Camera_rotation_x = 30  # radiansToDegree(np.arctan(1/np.sqrt(6)))
Camera_rotation_y = 30  # -radiansToDegree(np.arctan(np.sqrt(3) / 2))  # -45
Camera_rotation_z = 0

f = 35  # 焦距mm8
Cmos_x = 23.76  # CMOS宽度mm
Cmos_y = 13.365  # CMOS长度mm
Pix_x = 1920  # 像素
Pix_y = 1080  # 像素
u0 = 960  #
v0 = 540  #

# 世界坐标系中的坐标点
Xw = 0
Yw = 0
Zw = 0
W = np.mat([[Xw], [Yw], [Zw], [1]])

# R1 = getRotationMatrix(degreeToRadians(90), degreeToRadians(90), degreeToRadians(0))
# R2 = getRotationMatrix(degreeToRadians(-30), degreeToRadians(-45), degreeToRadians(0))
R2 = getRotationMatrix(degreeToRadians(Camera_rotation_x),
                       degreeToRadians(Camera_rotation_y),
                       degreeToRadians(Camera_rotation_z))

RT = np.hstack((R2, -R2 * np.mat([[Camera_position_x], [Camera_position_y], [Camera_position_z]])))  # 外参

M2 = np.vstack((RT, [0, 0, 0, 1]))

M1 = np.mat([[f / (Cmos_x / Pix_x), 0, u0, 0], [0, f / (Cmos_y / Pix_y), v0, 0], [0, 0, 1, 0]])  # 内参

Re = M1 * M2 * W
print(Re)
print("W:", Re[0][0] / Re[2][0])
print("H:", Re[1][0] / Re[2][0])
